6DoF Cooperative Localization for Mutually Observing Robots

The solution of cooperative localization is of particular importance to teams of aerial or underwater robots operating in areas devoid of landmarks. The problem becomes harder if the localization system must be low-cost and lightweight enough that only consumer-grade cameras can be used. This paper presents an analytical solution to the six degrees of freedom cooperative localization problem using bearing only measurements. Given two mutually observing robots, each one equipped with a camera and two markers, and given that they each take a picture at the same moment, we can recover the coordinate transformation that expresses the pose of one robot in the frame of reference of the other. The novelty of our approach is the use of two pairs of bearing measurements for the pose estimation instead of using both bearing and range measurements. The accuracy of the results is verified in extensive simulations and in experiments with real hardware. At 6.5 m distance, position was estimated with a mean error between 0.021 m and 0.025 m and orientation was recovered with a mean error between 0.019 rad and 0.037 rad. This makes our solution particularly well suited for deployment on fleets of inexpensive robots moving in 6 DoF such as blimps.